package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;


public class PickingTreasureSequence extends RobotRunningSequence {
	public static int SMALL_TURN_INDEX = 2; // Degree
	
	public static int MOVE_DISTANCE = 15;
	
	/**
	 * Alignment threshold beyond which we should adjust the orientation of the robot.
	 */
	private final static double ALIGNMENT_THRESHOLD = 5;
	
	public static final int Move_Steps = 15;

	enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.log("Start picking treasure", 2);

				IDetector detector = robot.getTreasureDetector();
				
				if (detector.isPresent()) {
					int d = RobotRunningSequence.getOrientation(robot, detector);
					
					String log = String.format("Turning degree: %d", d);
					robot.log(log, 4);

					if (Math.abs(d) > 2) {
						robot.sendCommand(RobotAction.TurnDegree, d);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					} 

					machine.changeState(ArmOpen);
				}
				else {
					machine.changeState(TreasureLost);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		OrientationPositioning {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Orientation positioning", 4);
				
				machine.robot().sendCommand(RobotAction.Stop);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				/*
				// TODO Auto-generated method stub
				if (arm_up==1){
					machine.robot().sendCommand(RobotAction.MoveBackward, Move_Steps);
					machine.changeState(ArmDownOpen);
				}
				else{
					machine.robot().sendCommand(RobotAction.MoveForward, Move_Steps);
					machine.changeState(Picking);
				}
				*/
				IDetector detector = machine.robot().getTreasureDetector();
				
				//int deviationThres = (int) (detector.getWidth() / 2 - center.x);
				if (detector.isPresent()) {
					int d = RobotRunningSequence.getOrientation(machine.robot(), detector);
					
					String log = String.format("Turning degree: %d", d);
					robot.log(log, 4);

					if (Math.abs(d) > ALIGNMENT_THRESHOLD) {
						robot.sendCommand(RobotAction.TurnDegree, d);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					} 
					else {
						robot.log("Orientation positioning done", 2);
						
						machine.changeState(ArmOpen);
					}
				}
				else {
					machine.changeState(TreasureLost);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		ArmOpen {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Arm down", 2);

				machine.robot().sendCommand(RobotAction.ArmDown);
				machine.robot().sleep(Robot.MOTION_SETTLING_TIME);
				
				//change armdown so that it pulses in reverse for a short time, then pulses x cycles to drop arm and open fully
				machine.changeState(DistancePositioning);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		DistancePositioning {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Distance positioning", 3);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				/*
				IDetector detector = robot.getTreasureDetector();
				
				if (detector.isPresent()) {
					// move in small step until the treasure is missing from the screen
					robot.log(String.format("Move %d", SMALL_MOVE_INDEX), 3);
					
					robot.sendCommand(RobotAction.Move, SMALL_MOVE_INDEX);
				}
				else {
					robot.log("Distance positioning done", 3);
					
					machine.changeState(Picking);
				}
				*/
				robot.log(String.format("Move %d", MOVE_DISTANCE), 3);
				
				robot.sendCommand(RobotAction.MoveToTreasure, MOVE_DISTANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				//robot.sendCommand(RobotAction.Stop);
				
				machine.changeState(Picking);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Picking {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.log("Pick treasure comment sent", 3);
				
				robot.sendCommand(RobotAction.ArmUp);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
						//need to update robot commander pick()
						//send pulse
				machine.changeState(Picked);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		CheckPicked {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Check if treasure picked", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				IDetector detector = machine.robot().getTreasureDetector();
				
				if (detector.isPresent()==false) {
					machine.changeState(Picked);
				}
				else {
					machine.changeState(TreasureLost);
				}
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Picked {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Treasure picked", 2);
				
				machine.robot().onTreasurePicked();
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		TreasureLost {

			@Override
			public void enter(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.log("Treasure lost", 2);
				
				robot.sendCommand(RobotAction.MoveToTreasure, -10);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		};
		static PickingTreasureSequence m_sequence;
		
		public static void setSequence(PickingTreasureSequence sequence) {
			m_sequence = sequence;
		}
	}
	
	public PickingTreasureSequence(RobotRunningStateMachine running) {
		super(running, State.Start);
		State.setSequence(this);
		m_currState = State.Start;
	}

	@Override
	public void start() {
		Robot robot = m_running.robot();
		//robot.log("Picking treasure", 1);
		
		m_currState = State.Start;
		m_nextState = null;
		
		robot.enableTreasureDetector(true);
		//robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(false);
		robot.enableMaze2Detector(false);
		robot.enableMaze3Detector(false);
		robot.enableMazeEntranceGuideDetector(false);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(true);
	}
	
	@Override
	public boolean succeeded() {
		return m_currState.equals(State.Picked);
	}

	@Override
	public boolean failed() {
		return m_currState.equals(State.TreasureLost);
	}

}
